#ifndef __mml_Matrix_h__
#define __mml_Matrix_h__

#include <mml/Vector.h>

#include <iomanip>

namespace mml {
  /**
  \class Matrix mml/Matrix.h
  \brief Matrix template class.
  \ingroup math
  */

  template <int M, int N, typename Type>
  class Matrix
  {
    Type m_data[M * N];

  public:
    /** Default constructor. */
    Matrix()
    {
      vector_reset(M * N, m_data);
    }

    /** Copy constructor. */
    Matrix(Matrix const& other)
    {
      vector_copy(M * N, m_data, other.m_data);
    }

    /** Destructor. */
    ~Matrix()
    {}

    /** Copy assignment operator. */
    Matrix const& operator=(Matrix const& other)
    {
      vector_copy(M * N, m_data, other.m_data);
      return *this;
    }

    /** Scale a matrix */
    void scale(Type const& s)
    {
      vector_scale(M * N, m_data, s);
    }

    /** Add a matrix to this one */
    void add(Matrix const& other)
    {
      vector_add(M * N, m_data, other.m_data);
    }

    /** Subtract a matrix from this one */
    void subtract(Matrix const& other)
    {
      vector_subtract(M * N, m_data, other.m_data);
    }

    /** Negate the matrix. */
    void negate()
    {
      vector_negate(M * N, m_data);
    }

    /** Internal data. */
    Type const* getData() const
    {
      return m_data;
    }

    /** Non-const internal data. */
    Type* getData()
    {
      return const_cast<Type*>(static_cast<Matrix<M, N, Type> const&>(*this).getData());
    }

    /** Index operator */
    Type const& operator()(int i, int j) const
    {
#ifdef _DEBUG
      if (i < 0 || i >= M || j < 0 || j >= N) {
        throw mml::IndexOutOfBounds();
      }
#endif
      return m_data[i * N + j];
    }

    /** Index operator */
    Type& operator()(int i, int j)
    {
      return const_cast<Type&>(static_cast<Matrix<M, N, Type> const&>(*this)(i, j));
    }

    /** Reset a matrix */
    void reset()
    {
      vector_reset(M * N, m_data);
    }

    /** Transpose */
    Matrix<N, M, Type> transpose() const
    {
      Matrix<N, M, Type> At;  // new matrix
      mml::matrix_transpose(M, N, m_data, At.getData());  // fill At
      return At;
    }

    /** Unit matrix. */
    static mml::Matrix<M, N, Type> unit()
    {
      mml::Matrix<M, N, Type> I;
      I.reset();
      for (int i = 0; i < mml::min(M, N); ++i) {
        I.m_data[N * i + i] = 1.0;
      }
      return I;
    }

    /** From Tait-Bryan angles. */
    static mml::Matrix<3, 3, Type> fromTaitBryan(double const& roll, double const& pitch, double const& yaw)
    {
      mml::Matrix<3, 3, Type> ret;

      // Precompute sines and cosines of roll, pitch and yaw:
      double cosroll = cos(roll);
      double sinroll = sin(roll);
      double cospitch = cos(pitch);
      double sinpitch = sin(pitch);
      double cosyaw = cos(yaw);
      double sinyaw = sin(yaw);

      // Fill in the matrix:
      ret(0, 0) = cospitch * cosyaw                                 ;  ret(0, 1) = cospitch * sinyaw                                 ;  ret(0, 2) = -sinpitch          ;
      ret(1, 0) = sinroll * sinpitch * cosyaw  -  cosroll * sinyaw  ;  ret(1, 1) = cosroll * cosyaw  -  sinroll * sinpitch * sinyaw  ;  ret(1, 2) =  sinroll * cospitch;
      ret(2, 0) = cosroll * sinpitch * cosyaw  +  sinroll * sinyaw  ;  ret(2, 1) = cosroll * sinpitch * sinyaw  -  sinroll * cosyaw  ;  ret(2, 2) =  cosroll * cospitch;

      return ret;
    }
  };

  // typedefs:
  typedef Matrix<2, 2, float> Matrix2f;
  typedef Matrix<2, 2, double> Matrix2d;
  typedef Matrix<3, 3, float> Matrix3f;
  typedef Matrix<3, 3, double> Matrix3d;
  typedef Matrix<4, 4, float> Matrix4f;
  typedef Matrix<4, 4, double> Matrix4d;
  typedef Matrix<6, 6, float> Matrix6f;
  typedef Matrix<6, 6, double> Matrix6d;


  /** Matrix vector multiplication */
  template<int M, int N, typename Type>
  Vector<M, Type> operator*(Matrix<M, N, Type> const& A, Vector<N, Type> const& x)
  {
    Vector<M, Type> y;
    matrix_matvecmul( M, N, A.getData(), x.getData(), y.getData() );
    return y;
  }

  /** Matrix-matrix multiplication */
  template<int M, int N, int O, typename Type>
  Matrix<M, O, Type> operator*(Matrix<M, N, Type> const& A, Matrix<N, O, Type> const& B)
  {
    Matrix<M, O, Type> C;
    matrix_matmatmul(M, N, O, A.getData(), B.getData(), C.getData());
    return C;
  }

  /** Serialization */
  template<int M, int N, typename Type>
  std::ostream& operator<<(std::ostream& o, mml::Matrix<M, N, Type> const& A)
  {
    for (int i = 0; i < M; ++i) {
      for (int j = 0; j < N; ++j) {
        o << std::setw(20) << A(i, j);
      }
      o << std::endl;
    }

    return o;
  }

  /** Solve a square system */
  template <int N, typename Type>
  Vector<N, Type> solve(Matrix<N, N, Type> const& A, Vector<N, Type> const& b)
  {
    Matrix<N, N, Type> A1(A);
    Vector<N, Type> x(b);
    mml::gauss(N, A1.getData(), x.getData());
    return x;
  }

  /** Solve a non-square system */
  template <int M, int N, typename Type>
  Vector<N, Type> solve(Matrix<M, N, Type> const& A, Vector<M, Type> const& b)
  {
    Matrix<M, N, Type> A1(A);
    Vector<M, Type> b1(b);
    Vector<N, Type> x;
    mml::gramschmidt(M, N, A1.getData(), b1.getData(), x.getData());
    return x;
  }

  /** In place addition */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> const& operator+=(Matrix<M, N, Type>& target, Matrix<M, N, Type> const& source)
  {
    target.add(source);
    return target;
  }

  /** In place subtraction */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> const& operator-=(Matrix<M, N, Type>& target, Matrix<M, N, Type> const& source)
  {
    target.subtract(source);
    return target;
  }

  /** Addition */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> operator+(Matrix<M, N, Type> const& t1, Matrix<M, N, Type> const& t2)
  {
    Matrix<M, N, Type> ret(t1);
    ret.add(t2);
    return ret;
  }

  /** Subtraction */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> operator-(Matrix<M, N, Type> const& t1, Matrix<M, N, Type> const& t2)
  {
    Matrix<M, N, Type> ret(t1);
    ret.subtract(t2);
    return ret;
  }

  /** In place scalar multiplication */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> const& operator*=(Matrix<M, N, Type>& t, Type const& s)
  {
    t.scale(s);
    return t;
  }

  /** Prefix scalar multiplication */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> operator*(Type const& s, Matrix<M, N, Type> const& t)
  {
    Matrix<M, N, Type> ret(t);
    ret.scale(s);
    return ret;
  }

  /** Postfix scalar multiplication */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> operator*(Matrix<M, N, Type> const& A, Type const& s)
  {
    Matrix<M, N, Type> ret(A);
    ret.scale(s);
    return ret;
  }

  /** In place scalar division */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> const& operator/=(Matrix<M, N, Type>& A, Type const& s)
  {
    A.scale(1.0 / s);
    return A;
  }

  /** Postfix scalar division */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> operator/(Matrix<M, N, Type> const& A, Type const& s)
  {
    Matrix<M, N, Type> ret(A);
    ret.scale(1.0 / s);
    return ret;
  }

  /** Vector outer product. */
  template<int M, int N, typename Type>
  Matrix<M, N, Type> outerProduct(Vector<M, Type> const& a, Vector<N, Type> const& b)
  {
    Matrix<M, N, Type> product;
    vector_outerProduct(M, N, a.getData(), b.getData(), product.getData());
    return product;
  }

  /** Unary minus */
  template <int M, int N, typename Type>
  Matrix<M, N, Type> operator-(Matrix<M, N, Type> const& matrix)
  {
    Matrix<M, N, Type> ret(matrix);
    ret.negate();
    return ret;
  }

}

#endif
